#include "FE.h"

////////////////////////////////////////////
//  calculate the parameters of nodal finite element
////////////////////////////////////////////
void FENodal(double *ae, double *be, double *ce, double *de,
	     double *Vole, double *v0, double *v1, double *v2, double *v3)
{
  int i;
  double Det = 0;
  double xe[4], ye[4], ze[4];

  xe[0] = v0[0]; xe[1] = v1[0]; xe[2] = v2[0]; xe[3] = v3[0];
  ye[0] = v0[1]; ye[1] = v1[1]; ye[2] = v2[1]; ye[3] = v3[1];
  ze[0] = v0[2]; ze[1] = v1[2]; ze[2] = v2[2]; ze[3] = v3[2];

  // calculate all coefficients for all basis
  ae[0] =  xe[1]*(ye[2]*ze[3]-ze[2]*ye[3])
    - xe[2]*(ye[1]*ze[3]-ze[1]*ye[3]) + xe[3]*(ye[1]*ze[2]-ze[1]*ye[2]);
  ae[1] = -xe[0]*(ye[2]*ze[3]-ze[2]*ye[3])
    + xe[2]*(ye[0]*ze[3]-ze[0]*ye[3]) - xe[3]*(ye[0]*ze[2]-ze[0]*ye[2]);
  ae[2] =  xe[0]*(ye[1]*ze[3]-ze[1]*ye[3])
    - xe[1]*(ye[0]*ze[3]-ze[0]*ye[3]) + xe[3]*(ye[0]*ze[1]-ze[0]*ye[1]);
  ae[3] = -xe[0]*(ye[1]*ze[2]-ze[1]*ye[2])
    + xe[1]*(ye[0]*ze[2]-ze[0]*ye[2]) - xe[2]*(ye[0]*ze[1]-ze[0]*ye[1]);

  be[0] = -(ye[2]*ze[3]-ze[2]*ye[3])
    + (ye[1]*ze[3]-ze[1]*ye[3]) - (ye[1]*ze[2]-ze[1]*ye[2]);
  be[1] =  (ye[2]*ze[3]-ze[2]*ye[3])
    - (ye[0]*ze[3]-ze[0]*ye[3]) + (ye[0]*ze[2]-ze[0]*ye[2]);
  be[2] = -(ye[1]*ze[3]-ze[1]*ye[3])
    + (ye[0]*ze[3]-ze[0]*ye[3]) - (ye[0]*ze[1]-ze[0]*ye[1]);
  be[3]= (ye[1]*ze[2]-ze[1]*ye[2])
    - (ye[0]*ze[2]-ze[0]*ye[2]) + (ye[0]*ze[1]-ze[0]*ye[1]);
  
  ce[0] =  (xe[2]*ze[3]-ze[2]*xe[3])
    - (xe[1]*ze[3]-ze[1]*xe[3]) + (xe[1]*ze[2]-ze[1]*xe[2]);
  ce[1] = -(xe[2]*ze[3]-ze[2]*xe[3])
    + (xe[0]*ze[3]-ze[0]*xe[3]) - (xe[0]*ze[2]-ze[0]*xe[2]);
  ce[2] =  (xe[1]*ze[3]-ze[1]*xe[3])
    - (xe[0]*ze[3]-ze[0]*xe[3]) + (xe[0]*ze[1]-ze[0]*xe[1]);
  ce[3] = -(xe[1]*ze[2]-ze[1]*xe[2])
    + (xe[0]*ze[2]-ze[0]*xe[2]) - (xe[0]*ze[1]-ze[0]*xe[1]);

  de[0] = -(xe[2]*ye[3]-ye[2]*xe[3])
    + (xe[1]*ye[3]-ye[1]*xe[3]) - (xe[1]*ye[2]-ye[1]*xe[2]);
  de[1] =  (xe[2]*ye[3]-ye[2]*xe[3])
    - (xe[0]*ye[3]-ye[0]*xe[3]) + (xe[0]*ye[2]-ye[0]*xe[2]);
  de[2] = -(xe[1]*ye[3]-ye[1]*xe[3])
    + (xe[0]*ye[3]-ye[0]*xe[3]) - (xe[0]*ye[1]-ye[0]*xe[1]);
  de[3] =  (xe[1]*ye[2]-ye[1]*xe[2])
    - (xe[0]*ye[2]-ye[0]*xe[2]) + (xe[0]*ye[1]-ye[0]*xe[1]);

  // return the coefficient for linear nodal fe 
  Det = (ae[0] + ae[1] + ae[2] + ae[3]);
  for(i = 0; i < 4; i++){ 
    ae[i] /= Det; 
    be[i] /= Det; 
    ce[i] /= Det; 
    de[i] /= Det;
  }
  // retun the area of current tetrahedron
  (*Vole) = fabs(Det)/6.0;

}


/**
 * subroutines for Linear Edge Finite Element
 * Each point and returned val are 3-valued vector
 *
 * Test passed on 18 June.
 * need to coporated with EdgeSign
 *
 */
void FEEdge(double *bas_val, 
	    double *curl_bas_val,    // curl is also constant
	    double *qp, int n_pts,   // logical node in master element
	    double *jacobian,
	    double *v0, double *v1, double *v2, double *v3)
{
  int i,j,l,i1,i2;
  double x,y,z;
  double xe[4], ye[4], ze[4];
  double ae[4], be[4], ce[4], de[4];
  double L[4], *val, det, vol;

  xe[0] = v0[0]; xe[1] = v1[0]; xe[2] = v2[0]; xe[3] = v3[0];
  ye[0] = v0[1]; ye[1] = v1[1]; ye[2] = v2[1]; ye[3] = v3[1];
  ze[0] = v0[2]; ze[1] = v1[2]; ze[2] = v2[2]; ze[3] = v3[2];

  // calculate all coefficients for all basis
  ae[0] =  xe[1]*(ye[2]*ze[3]-ze[2]*ye[3])
    - xe[2]*(ye[1]*ze[3]-ze[1]*ye[3]) + xe[3]*(ye[1]*ze[2]-ze[1]*ye[2]);
  ae[1] = -xe[0]*(ye[2]*ze[3]-ze[2]*ye[3])
    + xe[2]*(ye[0]*ze[3]-ze[0]*ye[3]) - xe[3]*(ye[0]*ze[2]-ze[0]*ye[2]);
  ae[2] =  xe[0]*(ye[1]*ze[3]-ze[1]*ye[3])
    - xe[1]*(ye[0]*ze[3]-ze[0]*ye[3]) + xe[3]*(ye[0]*ze[1]-ze[0]*ye[1]);
  ae[3] = -xe[0]*(ye[1]*ze[2]-ze[1]*ye[2])
    + xe[1]*(ye[0]*ze[2]-ze[0]*ye[2]) - xe[2]*(ye[0]*ze[1]-ze[0]*ye[1]);

  be[0] = -(ye[2]*ze[3]-ze[2]*ye[3])
    + (ye[1]*ze[3]-ze[1]*ye[3]) - (ye[1]*ze[2]-ze[1]*ye[2]);
  be[1] =  (ye[2]*ze[3]-ze[2]*ye[3])
    - (ye[0]*ze[3]-ze[0]*ye[3]) + (ye[0]*ze[2]-ze[0]*ye[2]);
  be[2] = -(ye[1]*ze[3]-ze[1]*ye[3])
    + (ye[0]*ze[3]-ze[0]*ye[3]) - (ye[0]*ze[1]-ze[0]*ye[1]);
  be[3]= (ye[1]*ze[2]-ze[1]*ye[2])
    - (ye[0]*ze[2]-ze[0]*ye[2]) + (ye[0]*ze[1]-ze[0]*ye[1]);
  
  ce[0] =  (xe[2]*ze[3]-ze[2]*xe[3])
    - (xe[1]*ze[3]-ze[1]*xe[3]) + (xe[1]*ze[2]-ze[1]*xe[2]);
  ce[1] = -(xe[2]*ze[3]-ze[2]*xe[3])
    + (xe[0]*ze[3]-ze[0]*xe[3]) - (xe[0]*ze[2]-ze[0]*xe[2]);
  ce[2] =  (xe[1]*ze[3]-ze[1]*xe[3])
    - (xe[0]*ze[3]-ze[0]*xe[3]) + (xe[0]*ze[1]-ze[0]*xe[1]);
  ce[3] = -(xe[1]*ze[2]-ze[1]*xe[2])
    + (xe[0]*ze[2]-ze[0]*xe[2]) - (xe[0]*ze[1]-ze[0]*xe[1]);

  de[0] = -(xe[2]*ye[3]-ye[2]*xe[3])
    + (xe[1]*ye[3]-ye[1]*xe[3]) - (xe[1]*ye[2]-ye[1]*xe[2]);
  de[1] =  (xe[2]*ye[3]-ye[2]*xe[3])
    - (xe[0]*ye[3]-ye[0]*xe[3]) + (xe[0]*ye[2]-ye[0]*xe[2]);
  de[2] = -(xe[1]*ye[3]-ye[1]*xe[3])
    + (xe[0]*ye[3]-ye[0]*xe[3]) - (xe[0]*ye[1]-ye[0]*xe[1]);
  de[3] =  (xe[1]*ye[2]-ye[1]*xe[2])
    - (xe[0]*ye[2]-ye[0]*xe[2]) + (xe[0]*ye[1]-ye[0]*xe[1]);

  // volume of tetrahedron
  det = (ae[0] + ae[1] + ae[2] + ae[3]);
  for(i = 0; i < 4; i++){
    ae[i] /= det;
    be[i] /= det;
    ce[i] /= det;
    de[i] /= det;
  }
  (*jacobian) = det; // return the jacobian for linear triangle

  // DL is constants on each Tet
  /* DL[0][0] = -det*((ye[2]-ye[1])*(ze[3]-ze[1]) - (ze[2]-ze[1])*(ye[3]-ye[1])); */
  /* DL[0][1] =  det*((xe[2]-xe[1])*(ze[3]-ze[1]) - (ze[2]-ze[1])*(xe[3]-xe[1])); */
  /* DL[0][2] = -det*((xe[2]-xe[1])*(ye[3]-ye[1]) - (ye[2]-ye[1])*(xe[3]-xe[1])); */

  /* DL[1][0] =  det*((ye[2]-ye[0])*(ze[3]-ze[0]) - (ze[2]-ze[0])*(ye[3]-ye[0])); */
  /* DL[1][1] = -det*((xe[2]-xe[0])*(ze[3]-ze[0]) - (ze[2]-ze[0])*(xe[3]-xe[0])); */
  /* DL[1][2] =  det*((xe[2]-xe[0])*(ye[3]-ye[0]) - (ye[2]-ye[0])*(xe[3]-xe[0])); */

  /* DL[2][0] = -det*((ye[1]-ye[0])*(ze[3]-ze[0]) - (ze[1]-ze[0])*(ye[3]-ye[0])); */
  /* DL[2][1] =  det*((xe[1]-xe[0])*(ze[3]-ze[0]) - (ze[1]-ze[0])*(xe[3]-xe[0])); */
  /* DL[2][2] = -det*((xe[1]-xe[0])*(ye[3]-ye[0]) - (ye[1]-ye[0])*(xe[3]-xe[0])); */

  /* DL[3][0] =  det*((ye[1]-ye[0])*(ze[2]-ze[0]) - (ze[1]-ze[0])*(ye[2]-ye[0])); */
  /* DL[3][1] = -det*((xe[1]-xe[0])*(ze[2]-ze[0]) - (ze[1]-ze[0])*(xe[2]-xe[0])); */
  /* DL[3][2] =  det*((xe[1]-xe[0])*(ye[2]-ye[0]) - (ye[1]-ye[0])*(xe[2]-xe[0])); */

  /* DL[0][0] = be[0]*det; DL[0][1] = ce[0]*det; DL[0][2] = de[0]*det; */
  /* DL[1][0] = be[1]*det; DL[1][1] = ce[1]*det; DL[1][2] = de[1]*det; */
  /* DL[2][0] = be[2]*det; DL[2][1] = ce[2]*det; DL[2][2] = de[2]*det; */
  /* DL[3][0] = be[3]*det; DL[3][1] = ce[3]*det; DL[3][2] = de[3]*det; */


  // curl of basis is element-wise constant
  double len[6];
  for(j = 0; j < 6; j++){
    i1 = Edge_end[j][0]; 
    i2 = Edge_end[j][1];

    // N_ij = (L_i \nabla L_j - L_j \nabla L_i)*|e_ij| 
    len[j] = sqrt((xe[i2] - xe[i1])*(xe[i2] - xe[i1]) +
		  (ye[i2] - ye[i1])*(ye[i2] - ye[i1]) +
		  (ze[i2] - ze[i1])*(ze[i2] - ze[i1]));

    curl_bas_val[3*j + 0] = 2.0*len[j]*(ce[i1]*de[i2] - de[i1]*ce[i2]);
    curl_bas_val[3*j + 1] = 2.0*len[j]*(de[i1]*be[i2] - be[i1]*de[i2]);
    curl_bas_val[3*j + 2] = 2.0*len[j]*(be[i1]*ce[i2] - ce[i1]*be[i2]);

    // set base value for all 6 basis
    for(l = 0; l < n_pts; l++){
      // The base value of nodal function at this point
      L[1] = qp[l*3 + 0]; 
      L[2] = qp[l*3 + 1]; 
      L[3] = qp[l*3 + 2];
      L[0] = 1 - L[1] - L[2] - L[3];

      bas_val[18*l + 3*j + 0] = (L[i1]*be[i2] - L[i2]*be[i1])*len[j];
      bas_val[18*l + 3*j + 1] = (L[i1]*ce[i2] - L[i2]*ce[i1])*len[j];
      bas_val[18*l + 3*j + 2] = (L[i1]*de[i2] - L[i2]*de[i1])*len[j];

    }// finish all 6 edges
    //   printf("\n");

  } // finshi all quadrature points
  
}


/**
 * local quadrature point (qp[0], qp[1], qp[2])
 * in Element < vtx[0] ~ vtx[3] >
 * The master element looks like: with its volumn being 1/6.
 *    v3          
 *     ^        v2
 *     |       ^ 
 *     |      /           
 *     |    /
 *     |  /
 *     v0 ---------> v1
 *
 * Test passed on 18 June.
 */
void local_to_global(double *qp_global, double *qp_local, int n_pts, 
		     double *v0, double *v1, double *v2, double *v3)
{
  int i;
  double L[4];
  for(i = 0; i < n_pts; i++){
    // local barycentric coordiante of point i
    L[1] = qp_local[3*i + 0];
    L[2] = qp_local[3*i + 1];
    L[3] = qp_local[3*i + 2];
    L[0] = 1 - L[1] - L[2] - L[3];

    qp_global[i*3 + 0] = L[0]*v0[0] + L[1]*v1[0] + L[2]*v2[0] + L[3]*v3[0];
    qp_global[i*3 + 1] = L[0]*v0[1] + L[1]*v1[1] + L[2]*v2[1] + L[3]*v3[1];
    qp_global[i*3 + 2] = L[0]*v0[2] + L[1]*v1[2] + L[2]*v2[2] + L[3]*v3[2];
  }
}


// get quadrature information, 
// read from txt files, which is expandable
void quad_rule_tet(double **qp, double **qw, int *n_pts, int order)
{
  int i;
  double x,y,z,w;
  char quad_file[20];
  FILE *fin;

  sprintf(quad_file, "QuadRule.tet.%d", order);
  fin = fopen(quad_file, "r");
  
  fscanf(fin, "%d", n_pts);
  //  printf("You choice %d points quadrature rule!\n", *n_pts);

  (*qw) = (double *) malloc((*n_pts)*  sizeof(double));
  (*qp) = (double *) malloc((*n_pts)*3*sizeof(double));
  
  for(i = 0; i < (*n_pts); i++){
    fscanf(fin, "%lf %lf %lf %lf", &x, &y, &z, &w);
    (*qw)[i] = w;
    (*qp)[3*i + 0] = x;
    (*qp)[3*i + 1] = y;
    (*qp)[3*i + 2] = z;
  }

  fclose(fin);

}
